//
// Created by ze on 2023/11/9.
//
#include "UPRE_ROBOT_INIT.h"

#include "bsp_fbcf.h"
#include "bsp_uart.h"
#include "UPRE_GYRO.h"
#include "slowStart.h"
#include "Task scheduling_l.h"
#include "usart.h"
#include "ultrasonic.h"
#include "tim.h"
#include "UPRE_BSP_WiFi.h"
#include "UPRE_DIJKSTRA.h"
#include "UPRE_LOCATION.h"
#include "UPRE_PID.h"
#include "bsp_can.h"

uint8_t Rec_Uart1;

#define RXBUFFERSIZE  256

uint8_t RxBuffer_Uart1[RXBUFFERSIZE];
uint8_t Uart1_Rx_Cnt = 0;

void Robot_Init() {
    fdcan_init();
    // for(int i=0;i<=50;i++) {
        // slowStart_allopen(&hfdcan2, 0x02);
        // HAL_Delay(1);
    // }
    GYRO_A100_Init();
    HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
    WiFiRecevice_Init();
    Location_Init();
    pidInit();
    Dijkstra_FirstPathPlanning(33);
    //舵轮初始化
    // DJI_DeInit();
    // for (int i = 0; i < 10; ++i) {
        // slowStart_allopen(&hfdcan2, 0x04);
        // HAL_Delay(1);
    // }

// #if STEERING_LOCK == 1
//     fbcf_arm(0x1);
//     HAL_Delay(1);
//     fbcf_arm(0x3);
//     HAL_Delay(1);
//     fbcf_arm(0x2);
//     HAL_Delay(1000);
// #endif
}

int _write(int file, char *data, int len) {
    HAL_UART_Transmit_DMA(&huart7, (uint8_t *) data, len);
    // HAL_UART_Transmit(&huart7, (uint8_t *) data, len, 0xffff);
    return len;
}

extern bool once_togo;

Pose testPose = {0};
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart->Instance == UART7) {
        HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
        if (Uart1_Rx_Cnt >= 255) //婧㈠嚭鍒ゆ柇
        {
            Uart1_Rx_Cnt = 0;
            memset(RxBuffer_Uart1, 0x00, sizeof(RxBuffer_Uart1));
            // HAL_UART_Transmit(&huart1, (uint8_t *) "data_error\r\n", 12, 0xFFFF);
        } else {
            RxBuffer_Uart1[Uart1_Rx_Cnt++] = Rec_Uart1;

            if ((RxBuffer_Uart1[Uart1_Rx_Cnt - 1] == 0x0A) && (RxBuffer_Uart1[Uart1_Rx_Cnt - 2] == 0x0D)) {
                // for (int i = 0; i < Uart4_Rx_Cnt; ++i) {
                //     if (RxBuffer_Uart4[i] == 0x4f && RxBuffer_Uart4[i + 1] == 0x48) {
                //         flag_wifiFinish = true;
                //     }
                // }
                // printf("_1\r\n");
                // // HAL_UART_Transmit(&huart4, (uint8_t *) &RxBuffer_Uart1, Uart1_Rx_Cnt, 0xFFFF);
                // if ((RxBuffer_Uart1[Uart1_Rx_Cnt - 3] == 0x22)) {
                //     testPose.yaw += 90.f;
                // }
                if ((RxBuffer_Uart1[Uart1_Rx_Cnt - 3] == 0x44)) {
                    // testPose.yaw -= 90.f;
                    once_togo = true;
                }
                if ((RxBuffer_Uart1[Uart1_Rx_Cnt - 3] == 0x55)) {
                    Main_Control.task_state = RxBuffer_Uart1[Uart1_Rx_Cnt - 4];
                }

                while (HAL_UART_GetState(&huart7) == HAL_UART_STATE_BUSY_TX);
                Uart1_Rx_Cnt = 0;
                memset(RxBuffer_Uart1, 0x00, sizeof(RxBuffer_Uart1)); //娓呯┖鏁扮粍
            }
        }
        HAL_UART_Receive_IT(&huart7, &Rec_Uart1, 1);
    }

    if (huart->Instance == UART8) {
        // WiFi_Recevice();
        WiFi_Recevice_new();
        HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
    }
    if (huart->Instance == USART6) {
        HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
        get_gyroValue();
    }
}
